#include "var.h"
#include "MCF52259_PWM_driver.h"
unsigned long CT_INT_PIT0;
unsigned long CT_INT_UART0;
unsigned long CT_INT_UART1;
unsigned long CT_INT_ADC0;
unsigned long CT_INT_ADC1;
unsigned long CT_INT_ADCRANGE;
unsigned long TEST_SOLVE_CT;

unsigned long SPDA0;
unsigned long SPDA1;
unsigned long SPDB0;
unsigned long SPDB1;

unsigned char lastreadbyte;

float Offset_P ;
float Offset_D ;
float MAXDC_CONTROL;
float Degree_Distance_MID;
float Pre_offset;
float This_offset;
int TargetSPD;
float TSPD,TSPDA,TSPDB,SPD;
float SPDACC;
int MAXOFFSET;
float Pre_Control_Speed,Control_Speed;
float Degree_Distance;
float gyroMID,gyro;
float DCOffset,DC_Control;
float DC1,DC2,DC3;
float STAND_P,STAND_D,SPEED_P,SPEED_D;
float SPDlimit;
float tmpOFFSET;
float This_DC_Offset,Pre_DC_Offset;
float turningbalance;
float DC_LIM;
float DC_Control_D;
void initvalue()
{
	STAND_P=2800/100.0;
	STAND_D=1400/10000.0;
	SPEED_P=8000/10000.0;
	SPEED_D=200/100.0;
	Offset_P=30/100000.0;
	Offset_D=2815/10000.0;
	Degree_Distance_MID=2780;
	TargetSPD=430;
	
	turningbalance=7700/10000.0;
	
	SPDlimit=0;//180;
	MAXDC_CONTROL=10000;
	
}


void solve(){
int send;
  TEST_SOLVE_CT++;
  
  //////////////////////////////////////////SPEED CONTROL
  TargetSPD=444-(This_DC_Offset>0?This_DC_Offset:0.0-This_DC_Offset)*0.0000065;
  This_offset+= (TargetSPD-SPD)*SPEED_P;
  Degree_Distance_MID=2690-1.20*(TargetSPD-SPD);
  if(This_offset>SPDlimit)
  {
  	This_offset=SPDlimit;
  }
  else if(This_offset<-SPDlimit)
  {
  	This_offset=-SPDlimit;
  }
  Pre_DC_Offset=This_DC_Offset;
  This_DC_Offset=DC1-DC3;//DC1-DC3;
  
  //DC_Control_D=(DC_Control_D*19+This_DC_Offset-Pre_DC_Offset)/20;

  ///---------------------------------------SPEED CONTROL
  
   
  tmpOFFSET=SPEED_D*(TargetSPD-SPD);
  if(tmpOFFSET>1200)
  	tmpOFFSET=1200;
  else if(tmpOFFSET<-1200)
  	tmpOFFSET=-1200;
  Control_Speed =
  STAND_P*(Degree_Distance-Degree_Distance_MID+This_offset)-STAND_D*(gyro-gyroMID)
  +tmpOFFSET;

  DC_Control=Offset_P*This_DC_Offset+Offset_D*DC_Control_D;     
  
  if(Degree_Distance>6000||Degree_Distance<1000)
    Control_Speed=0;
  
  if((Control_Speed!=0)&&(SWITCH & 0x01)){
    if(Control_Speed>15000)
      Control_Speed=15000;
    if(Control_Speed<-15000)
      Control_Speed=-15000;
    
  }else{
    Control_Speed=1;
  }
  if(TSPD*DC_Control>MAXDC_CONTROL)
  {
  	setspdB(Control_Speed+MAXDC_CONTROL);
  	setspdA(Control_Speed-MAXDC_CONTROL*turningbalance);
  }
  else if(TSPD*DC_Control<-MAXDC_CONTROL)
  {
  	setspdB(Control_Speed-MAXDC_CONTROL*turningbalance);
  	setspdA(Control_Speed+MAXDC_CONTROL);
  }
  else
  {
  	
  	if(TSPD*DC_Control>0)
  	{
  		setspdB(Control_Speed+TSPD*DC_Control);
  		setspdA(Control_Speed-TSPD*DC_Control*turningbalance);
  		
  	}
  	else
  	{
  			
  		setspdB(Control_Speed+TSPD*DC_Control*turningbalance);
  		setspdA(Control_Speed-TSPD*DC_Control);
  		
  	}
  	
  }

}
